#include "chrono"
#include "opencv2/opencv.hpp"
#include <fstream>
#include <sys/time.h> 
#include "camera_node.hpp"
#include "act_D455.h"
#include "yolov8.hpp"
#include "pcl_process.hpp"
#include "trajectory_prediction.hpp"


#include <iostream>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/console/parse.h>


// 用于指示是否应该退出程序的全局标志
atomic<bool> should_exit(false);
// std::ofstream Show_data;
void sigint_handler(int signum) {
  cout << "Received SIGINT signal (" << signum << ")" << endl;
  should_exit = true;
}

int main(int argc, char* argv[]) {
    signal(SIGINT, sigint_handler);// 注册 SIGINT 信号处理程序
	// sleep(5);

	#ifdef TXT
		if (NULL == opendir("./data/")) {
			::mkdir("./data", 0777);
		}
		if (NULL == opendir("./data/camera")) {
			::mkdir("./data/camera", 0777);
		}
		if (NULL == opendir("./data/pics")) {
			::mkdir("./data/pics", 0777);
		}
		if (NULL == opendir("./data/frame")) {
			::mkdir("./data/frame", 0777);
		}
		
		time_t timeReal;
		time(&timeReal);
		timeReal = timeReal + 8*3600;//	格林尼治标准时间+8个小时
		tm* t = gmtime(&timeReal);
		ofstream DataOut;
		DataOut.open("./data/camera/" + format("%.2d%.2d\n%.2d:%.2d", t->tm_mon + 1, t->tm_mday, t->tm_hour, t->tm_min) + "\nrecord.txt");
		
		// Show_data.open("./data/camara_Show_data/" + format("%.2d%.2d\n%.2d:%.2d", t->tm_mon + 1, t->tm_mday, t->tm_hour, t->tm_min) + "\nrecord.txt");
		// Show_data<<"main in Show_data "<<endl;

		DataOut << "center_x" << "\t" 
				<< "center_y" << "\t" 
				<< "center_z" << "\t"
				<< "delta_time" << "\t" 
				<< "end_x" << "\t" 
				<< "end_z" << "\t" 
				<< "max_y" << "\t"
				// << "result_x_2" << "\t"
				<< "result_x_1" << "\t"
				<< "result_x_0" << "\t"
				<< "result_y_2" << "\t"
				<< "result_y_1" << "\t"
				<< "result_y_0" << "\t"
				// << "result_z_2" << "\t"/
				<< "result_z_1" << "\t"
				<< "result_z_0" << "\t"
				<< "yolo_flag" << "\t"
				<< "detect_flag" << "\t"
				<< "hit_flag" << "\t"
				<< "end_pitch" << "\t"
				<< "end_yaw" << "\t"
				<< "current_speed_x" << "\t"
				<< "current_speed_y" << "\t"
				<< "current_speed_z" << "\t"
				<< "time_sec" << "\t"
				<< "time_usec" << "\t"
				<< "time" << "\n";
	#endif


	
	cudaSetDevice(0);
    const std::string engine_file_path{"./src/camera_d455/modejson/2024_01_28.engine"};
    auto yolov8 = new YOLOv8(engine_file_path);// yolov8类初始化引擎路径
    yolov8->make_pipe(true);// 热身，对模型内存调用等进行初始化

	ActD455 D455;
	D455.Init();

	PclProcess pcl_process("roi_cloud");
	TraPredict trapredict;

	rclcpp::init(argc, argv);
	auto node = std::make_shared<CvNode>();



	cv::TickMeter tk;
	cv::TickMeter tk2;
	cv::TickMeter tk3;
	cv::TickMeter tk4;
	struct timeval time;
	Mat frame;
	int disp_flag = 0;


	while (rclcpp::ok() && !should_exit) {
		int code_flag = 0;

		//----------------------------------------------------------------------------------
		// 4.调用spin函数，并传入节点对象指针
  		rclcpp::spin_some(node);

		//----------------------------------------------------------------------------------
		// if(disp_flag == 0)
		// {
		// 	D455.Update();
		// 	disp_flag = 1;
		// }
		tk4.stop();
		cout << "------------------------每帧总时间:" << tk4.getTimeMilli() << endl;
		// DataOut << tk4.getTimeMilli() << endl;
		tk4.reset();
		tk4.start();

		D455.Update();
		gettimeofday(&time, NULL);

		resize(D455.GetSrcImage(), frame, Size(640, 480), 0, 0, INTER_AREA);
        tk.start();
		yolov8->detect(frame);
		tk.stop();
		cout << "------------------------detect time:" << tk.getTimeMilli() << endl;
		tk.reset();
		tk2.start();
		

		if (yolov8->resultFlag) {
			code_flag++;
			tk3.start();
			std::cout << "object.rect" <<yolov8->objs.at(0).rect << std::endl;

			if (D455.PointCloudGenerate(yolov8->objs.at(0).rect)) {
				tk3.stop();
				cout << "------------------------点云获取时间:" << tk3.getTimeMilli() << endl;
				tk3.reset();
				tk.start();
				pcl::copyPointCloud(*D455.GetSrcCloud(), *pcl_process.roi_cloud);
				// 获取球心坐标
				pcl_process.center_point = pcl_process.get_ball_center_ransac(pcl_process.roi_cloud, yolov8->objs.at(0).rect);
				// pcl_process.center_point = pcl_process.get_ball_center2(pcl_process.roi_cloud, D455.GetCenterPoint());
				tk.stop();
				cout << "------------------------球心拟合时间:" << tk.getTimeMilli() << endl;
				tk.reset();
				tk.start();
				// 获取轨迹方程
				trapredict.Ball_trajectory(pcl_process.center_point, time, trapredict.result_x, trapredict.result_y, trapredict.result_z);
				cout << "The coefficients vector is:	\n" << trapredict.result_x << "\n" 
				<< trapredict.result_y << "\n" << trapredict.result_z << endl;
				tk.stop();
				cout << "------------------------轨迹拟合时间:" << tk.getTimeMilli() << endl;
				tk.reset();
				tk.start();
				// 输出结果处理
				trapredict.Output_process(time, trapredict.result_x, trapredict.result_y, trapredict.result_z, trapredict.tra_output);
				// trapredict.tra_output.current_x = pcl_process.center_point.x;
				// trapredict.tra_output.current_y = pcl_process.center_point.y;
				// trapredict.tra_output.current_z = pcl_process.center_point.z;
				tk.stop();
				cout << "------------------------数据输出时间:" << tk.getTimeMilli() << endl;
				tk.reset();
			}

		}
		else{
			pcl_process.center_point.x = 0;
			pcl_process.center_point.y = 0;
			pcl_process.center_point.z = 0;
			trapredict.tra_output.clear();
		}

		tk2.stop();
		cout << "------------------------pcl and fit time:" << tk2.getTimeMilli() << endl;
		tk2.reset();
	
		node->publishCvData(trapredict.tra_output);

		namedWindow("result", WINDOW_NORMAL);
		cv::imshow("result", yolov8->res);

		pcl_process.Pcl_show();
		// pcl_process.Pcl_show(disp_flag); 显示指定文件夹内的点云

		int key = waitKey(1);
		if(key == 'p')
		{
			// pcl::copyPointCloud(*pcl_process.roi_cloud, *pcl_process.phase_cloud);
			// cout << cv::imwrite(format("./data/frame/%d_%d.jpg", time.tv_sec, time.tv_usec), frame) << endl; //保存图片
			disp_flag++;
		}
		if(key == 'o')
		{
			disp_flag--;
			// pcl_process.phase_cloud->points.clear();
		}
		

		#ifdef TXT
			DataOut << trapredict.tra_output.current_x << "\t" 
					<< trapredict.tra_output.current_y << "\t" 
					<< trapredict.tra_output.current_z << "\t"
					<< trapredict.tra_output.delta_time << "\t"
					<< trapredict.tra_output.end_x << "\t"
					<< trapredict.tra_output.end_z << "\t"
					<< trapredict.tra_output.max_y << "\t"
					// << trapredict.tra_output.result_x_2 << "\t"
					<< trapredict.tra_output.result_x_1 << "\t"
					<< trapredict.tra_output.result_x_0 << "\t"
					<< trapredict.tra_output.result_y_2 << "\t"
					<< trapredict.tra_output.result_y_1 << "\t"
					<< trapredict.tra_output.result_y_0 << "\t"
					// << trapredict.tra_output.result_z_2 << "\t"
					<< trapredict.tra_output.result_z_1 << "\t"
					<< trapredict.tra_output.result_z_0 << "\t"
					<< code_flag << "\t"
					<< trapredict.get_detect_flag() << "\t"
					<< HitFlag << "\t"
					<< trapredict.tra_output.end_pitch << "\t"
					<< trapredict.tra_output.end_yaw << "\t"
					<< trapredict.tra_output.current_speed_x << "\t"
					<< trapredict.tra_output.current_speed_y << "\t"
					<< trapredict.tra_output.current_speed_z << "\t"
					<< time.tv_sec << "\t"
					<< time.tv_usec << "\t"
					<< time.tv_sec % 1000 + int(time.tv_usec / 1000)*0.001 << "\n";
		#endif 

		pcl_process.cloud_point_clear();
	}

	cv::destroyAllWindows();


	// 5.释放资源
  	rclcpp::shutdown();
	return EXIT_SUCCESS;
}
